#ifndef __MY_UTILS_H__
#define __MY_UTILS_H__

#include <stdint.h>
#include <stdio.h>
#include <math.h>
#include <string>
#include <vector>

/**
 * @brief  S型速度曲线规划
 */
class SpeedUtils
{
private:
    int speed_max_; /// 最大速度，mm/s
    int speed_min_;
    int acc_max_;   /// 最大加速度，mm/s^2
    int jerk_;  /// 加加速度，每秒加速度增量， mm/s^3
    double delta;

    double overall;  /// 路径总长度，mm
    int cur_speed;  /// 当前条件下的速度
    int cur_acc_max;   /// 当前条件下的最大加速度
    int cur_acc;

    double choice[2];
    double time[8];
    double len[8];

    double len_sum(int start, int end);
public:
    SpeedUtils(int speed_max, int acc_max, int jerk = 100);
    ~SpeedUtils();
    void setGoal(double overall);
    int getSpeed(int walked);
};

std::vector<unsigned char> string2Bytes(std::string const &s);
std::string byte2String(unsigned char *recv, int start, int end);

#endif